from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
#封装终端指令相关类--------------
#from launch.actions import ExecuteProcess
#from launch.substitutions import FindExecutable
#参数声明与获取-----------------
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.substitutions import Command
#文件包含相关-------------------
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
#分组相关----------------------
#from launch_ros.actions import PushRosNamespace
#from launch.actions import GroupAction
#事件相关----------------------
from launch.event_handlers import OnProcessStart,OnProcessExit
from launch.actions import ExecuteProcess,RegisterEventHandler,LogInfo
#获取功能包下share目录路径-------
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    robot_name_in_model="fishbot"
    urdf_tutorial_path=get_package_share_directory("fishbot_description")
    default_model_path=urdf_tutorial_path+"/urdf/fishbot/fishbot.urdf.xacro"
    default_world_path=urdf_tutorial_path+"/world/map.world"
    
    action_declare_arg_mode_path=DeclareLaunchArgument(
        name="model",default_value=str(default_model_path),
        description="URDF的绝对路径")
    robot_description=ParameterValue(
        Command(["xacro ",LaunchConfiguration("model")]),
        value_type=str)
    robot_state_publisher_node=Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description":robot_description}]
    )
    # joint_state_publisher_node=Node(
    #     package="joint_state_publisher",
    #     executable="joint_state_publisher"  
    # )
    # rviz_node=Node(
    #     package="rviz2",
    #     executable="rviz2",
    #     arguments=["-d",get_package_share_directory("fishbot_description")+"/rviz/urdf.rviz"]
    # )
    launch_gazebo=IncludeLaunchDescription(
        PythonLaunchDescriptionSource([get_package_share_directory(
            "gazebo_ros"), "/launch","/gazebo.launch.py"]),
        launch_arguments=[("world",default_world_path),("verbose","true")]
    )
    spawn_entity_node=Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=["-topic","/robot_description",
                   "-entity",robot_name_in_model])
    
    load_joint_state_controller=ExecuteProcess(
        cmd=["ros2 control load_controller fishbot_joint_state_broadcaster",
             "--set-state active"],
             output="both",
             shell=True)
    # load_effort_controller=ExecuteProcess(
    #     cmd=["ros2 control load_controller fishbot_effort_controller",
    #          "--set-state active"],
    #          output="both",
    #          shell=True)
    load_diff_drive_controller=ExecuteProcess(
        cmd=["ros2 control load_controller fishbot_diff_drive_controller",
             "--set-state active"],
             output="both",
             shell=True)
    event_exit1=RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=spawn_entity_node,
            on_exit=load_joint_state_controller
        )
    )   
    # event_exit2=RegisterEventHandler(
    #     event_handler=OnProcessExit(
    #         target_action=load_joint_state_controller,
    #         on_exit=load_effort_controller
    #     )
    # )   
    event_exit3=RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=load_joint_state_controller,
            on_exit=load_diff_drive_controller
        )
    )      
    return LaunchDescription([
        action_declare_arg_mode_path,
        robot_state_publisher_node,
        launch_gazebo,
        spawn_entity_node,
        event_exit1,
        #event_exit2
        event_exit3
        ])